/** ***********************************************************************************
* @file         gsen_task.c
* @brief        加速度传感器数据处理任务
* @details      主要用来处理加速度传感器底层服务数，已经提供上传应用接口
* @author       杨小珛
* @date         2020/12/24
* @copyright    北京高知科技有限公司    
* @version      V1.0
* @attention    无
**************************************************************************************/
#include "gsen_task.h"
#include "mon_task.h"
#include "log_task.h"
#include "power_task.h"
#include "lis3dh_driver.h"
#include "bmi160_bsp.h"
#include "double_integral.h"

GaozSemaphoreHandle_t gsen_interrupe_semaphore; /*BLE连接状态信号量*/
GaozMutexHandle_t  gsen_get_data_mutex;          /*GSEN数据发送互斥体*/
gsen_bmi160_info_t gsen_bmi160_info;             /*定义GSEN数据结构体*/

gsen_bmi160_posture_t gsen_bmi160_p;              /* 车身姿态角分析*/   
gsen_bmi160_work_state_e gsen_work_st;            /* 加速度传感器的状态*/
gsen_bmi160_body_posture_t gsen_body_p;           /* 车身姿态分析*/  
gsen_state_fun_t gsen_state_fun;                  /* GSEN状态的回调*/
gsen_bmi160_body_state_e gsen_bmi160_body_st;     /* 车身状态*/  
volatile uint8_t gsen_shock_time;                 /* 加速度传感器连续振动的时间*/
volatile uint8_t gsen_power_man_id;               /*GSEN管理ID */

static void gsen_data_handle_task( void * pvParameters );
static int32_t gsen_iic_dev_init(void);

#if( GSEN_BMI160 == 1 ) 
struct bmi160_dev gsen_cfg;                      /*定义加速度传感器寄存器*/
static void gsen_delay_ms( uint32_t period );
static int8_t gsen_i2c_read(void *bus, uint8_t dev_addr, uint8_t reg_addr, uint8_t *data, uint16_t len);
static int8_t gsen_i2c_write(void *bus, uint8_t dev_addr, uint8_t reg_addr, uint8_t *data, uint16_t len);
static int8_t gsen_bmi160_config_work_mode(void);
#endif

static int8_t gsen_config_interrupt( uint8_t en );
static int8_t gsen_reg_init(void);
static void gsen_port_config_interrupt(void);
static void gsen_gpio_isr_cb( void *param );
static uint32_t gsen_sensor_data_get(void);
static uint32_t gsen_sensor_interrupt_data_get(void);
static void gens_interrupt_semaphore_create(void);
static uint8_t gens_interrupt_semaphore_get(void);
static void gens_interrupt_semaphore_set(void);

#if( GSEN_BMI160 == 1 ) 
static void gsen_bmi160_accel_value_conversion( struct bmi160_sensor_data accel );
//static void gsen_bmi160_gyro_value_conversion( struct bmi160_sensor_data gyro );
static void gsen_bmi160_sensor_euler_angle( gsen_bmi160_info_t msg );
static void gsen_bmi160_sensor_body_posture( gsen_bmi160_posture_t euler );
static void gsen_bmi160_sensor_body_crash( gsen_bmi160_body_posture_t *crash );
#endif

static void gsen_sensor_work_st_set( gsen_bmi160_work_state_e st );
static uint32_t gsen_sensor_state_cb( uint32_t st );
static uint32_t gsen_task_into_run_mode(void);
static uint32_t gsen_task_into_sleep_mode(void);
static uint32_t gsen_task_into_stop_mode(void);

#if( GSEN_LIS3DH == 1 ) 
static uint32_t gsen_lis3dh_accraw_get( int16_t *pdata );
static uint32_t gsen_lis3dh_sensitivity_get( uint8_t *sensitivity );
static uint32_t gsen_lis3dh_accvalue_get( AxesRaw_t *pdata );
static void gsen_lis3dh_accvalue_conversion( AxesRaw_t pdata );
static void gsen_lis3dh_sensor_body_posture( gsen_bmi160_posture_t euler );
static void gsen_lis3dh_sensor_euler_angle( gsen_bmi160_info_t pdata );
static void gsen_lis3dh_sensor_body_crash( gsen_bmi160_body_posture_t *crash );
#endif

/** ****************************************************************************
* @remarks       static void gsen_gpio_isr_cb( void *param )
* @brief         GSEN 外部中断口回调函数
* @param[in]     无
* @param[out]    无
* @return        = 0 成功 <0 失败
* @attention     无
*******************************************************************************/
static void gsen_gpio_isr_cb( void *param )
{
	hal_gpioisr_t *isr_cfg = (hal_gpioisr_t*)param;
	gaoz_gpio_get(GSEN_PORT_INTERRUPT_PIN,&(isr_cfg->level));
    system_wakeup_queue_set( SYS_WAKEUP_TRIG_GSEN );
    gens_interrupt_semaphore_set();
}

/** ****************************************************************************
* @remarks       static int32_t gsen_iic_dev_init(void)
* @brief         初始化IIC的驱动接口
* @param[in]     无
* @param[out]    无
* @return        = 0 成功 <0 失败
* @attention     无
*******************************************************************************/
static int32_t gsen_iic_dev_init(void)
{   
	HAL_I2C_BPS_T i2c_bsp_prt={
		.i2c_name = 1,
		.value = 0,
	};
    return gaoz_i2c_open( i2c_bsp_prt );
}

/** ****************************************************************************
* @remarks       static void gsen_port_config_interrupt(void)
* @brief         配置外部IO口中断
* @param[in]     无
* @param[out]    无
* @return        无
* @attention     无
*******************************************************************************/
static void gsen_port_config_interrupt(void)
{
    gaoz_gpio_pull_up_or_down( GSEN_PORT_INTERRUPT_PIN , true );
    oc_isr_t g_oc_ist_prt = 
    {       
        .is_debounce = true,
        .intr_enable = true,
        .intr_level = false,
        .intr_falling = true,
        .inte_rising = false, 
        .callback = gsen_gpio_isr_cb,
    };
	gaoz_gpio_isr_init( GSEN_PORT_INTERRUPT_PIN,&g_oc_ist_prt );	
}
#if( GSEN_BMI160 == 1 ) 
/** ****************************************************************************
* @remarks       static void gsen_delay_ms(uint32_t period)
* @brief         BMI160内部的延时程序
* @param[in]     period 延时周期，单位MS
* @param[out]    无
* @return        无
* @attention     无
*******************************************************************************/
static void gsen_delay_ms( uint32_t period )
{
    gaoz_task_sleep( period );
}

/** ****************************************************************************
* @remarks       static int8_t gsen_i2c_read(void *bus, uint8_t dev_addr, uint8_t reg_addr, uint8_t *data, uint16_t len)
* @brief         BMI160内部的 读数据
* @param[in]     period 延时周期，单位MS
* @param[out]    无
* @return        无
* @attention     无
*******************************************************************************/
static int8_t gsen_i2c_read(void *bus, uint8_t dev_addr, uint8_t reg_addr, uint8_t *data, uint16_t len)
{
	return i2c_get_data(dev_addr,reg_addr,false,data,len);
}

/** ****************************************************************************
* @remarks       static int8_t gsen_i2c_write(void *bus, uint8_t dev_addr, uint8_t reg_addr, uint8_t *data, uint16_t len)
* @brief         BMI160内部 写数据
* @param[in]     period 延时周期，单位MS
* @param[out]    无
* @return        无
* @attention     无
*******************************************************************************/
static int8_t gsen_i2c_write(void *bus, uint8_t dev_addr, uint8_t reg_addr, uint8_t *data, uint16_t len)
{
	return i2c_send_data(dev_addr,reg_addr,false,data,len);
}

/** ****************************************************************************
* @remarks       static int8_t gsen_bmi160_config_work_mode(void)
* @brief         配置BMI160的工作模式
* @param[in]     无
* @param[out]    无
* @return        BMI160_OK 初始化成功
* @attention     无
*******************************************************************************/
static int8_t gsen_bmi160_config_work_mode(void)
{
    int8_t rslt = BMI160_OK;
     
    /* Select the Output data rate, range of accelerometer sensor */
    gsen_cfg.accel_cfg.odr = BMI160_ACCEL_ODR_400HZ;
    gsen_cfg.accel_cfg.range = BMI160_ACCEL_RANGE_8G;
    gsen_cfg.accel_cfg.bw = BMI160_ACCEL_BW_NORMAL_AVG4;
    
    /* Select the power mode of accelerometer sensor */
    gsen_cfg.accel_cfg.power = BMI160_ACCEL_NORMAL_MODE;
        
    /* Select the Output data rate, range of Gyroscope sensor */
    gsen_cfg.gyro_cfg.odr = BMI160_GYRO_ODR_3200HZ;
    gsen_cfg.gyro_cfg.range = BMI160_GYRO_RANGE_2000_DPS;
    gsen_cfg.gyro_cfg.bw = BMI160_GYRO_BW_NORMAL_MODE;
         
    /* Select the power mode of Gyroscope sensor */
    gsen_cfg.gyro_cfg.power = BMI160_GYRO_NORMAL_MODE; 
    
    /* Set the sensor configuration */
    rslt = bmi160_set_sens_conf(&gsen_cfg);

    return rslt;
}

#endif

/** ****************************************************************************
* @remarks       static int8_t gsen_config_interrupt( uint8_t en )
* @brief         配置BMI160的中断模式
* @param[in]     无
* @param[out]    无
* @return        BMI160_OK 初始化成功
* @attention     无
*******************************************************************************/
static int8_t gsen_config_interrupt( uint8_t en )
{
	int8_t rslt = BMI160_OK;
#if( GSEN_LIS3DH == 1 ) 
    if( LIS3DH_SetInt1Threshold(   en ) != MEMS_SUCCESS )
    {   
        rslt = -1;
    }
#endif

#if( GSEN_BMI160 == 1 ) 
	struct bmi160_int_settg int_config;
    
	/* Select the Interrupt channel/pin */
	int_config.int_channel = BMI160_INT_CHANNEL_1;              // Interrupt channel/pin 1
    
	/* Select the Interrupt type */
	int_config.int_type = BMI160_ACC_ANY_MOTION_INT;            // Choosing Any motion interrupt
	/* Select the interrupt channel/pin settings */
	int_config.int_pin_settg.output_en = BMI160_ENABLE;         // Enabling interrupt pins to act as output pin
	int_config.int_pin_settg.output_mode = BMI160_DISABLE;      // Choosing push-pull mode for interrupt pin
	int_config.int_pin_settg.output_type = BMI160_DISABLE;      // Choosing active low output
	int_config.int_pin_settg.edge_ctrl = BMI160_ENABLE;         // Choosing edge triggered output
	int_config.int_pin_settg.input_en = BMI160_DISABLE;         // Disabling interrupt pin to act as input
	int_config.int_pin_settg.latch_dur = BMI160_LATCH_DUR_NONE;// non-latched output
    
	/* Select the Any-motion interrupt parameters */
	int_config.int_type_cfg.acc_any_motion_int.anymotion_en = BMI160_ENABLE;// 1- Enable the any-motion, 0- disable any-motion 
	int_config.int_type_cfg.acc_any_motion_int.anymotion_x = BMI160_ENABLE; // Enabling x-axis for any motion interrupt
	int_config.int_type_cfg.acc_any_motion_int.anymotion_y = BMI160_ENABLE; // Enabling y-axis for any motion interrupt
	int_config.int_type_cfg.acc_any_motion_int.anymotion_z = BMI160_ENABLE; // Enabling z-axis for any motion interrupt
	int_config.int_type_cfg.acc_any_motion_int.anymotion_dur = 3;           // any-motion duration
	int_config.int_type_cfg.acc_any_motion_int.anymotion_thr = en;          // (2-g range) -> (slope_thr) * 3.91 mg, (4-g range) -> (slope_thr) * 7.81 mg, (8-g range) ->(slope_thr) * 15.63 mg, (16-g range) -> (slope_thr) * 31.25 mg 
	
	/* Set the Any-motion interrupt */
	rslt = bmi160_set_int_config(&int_config, &gsen_cfg); /* sensor is an instance of the structure bmi160_dev  */
#endif
    return rslt;
}

/** ****************************************************************************
* @remarks       static int32_t gsen_reg_init(void)
* @brief         初始化 bmi160 寄存器
* @param[in]     无
* @param[out]    无
* @return        0 初始化成功       -1 初始化失败
* @attention     无
*******************************************************************************/
static int8_t gsen_reg_init(void)
{
#if( GSEN_LIS3DH == 1 ) 
    if( LIS3DH_SetODR(LIS3DH_ODR_400Hz) != MEMS_SUCCESS )
    {
        return -1;
    }
    if( LIS3DH_SetMode(LIS3DH_NORMAL) != MEMS_SUCCESS )
    {
        return -1;
    }
    if( LIS3DH_SetFullScale(LIS3DH_FULLSCALE_8) != MEMS_SUCCESS )
    {
        return -1;
    }
    if( LIS3DH_SetAxis(LIS3DH_X_ENABLE | LIS3DH_Y_ENABLE | LIS3DH_Z_ENABLE) != MEMS_SUCCESS )
    {
        return -1;
    }
    if( LIS3DH_SetHPFMode(LIS3DH_HPM_NORMAL_MODE) != MEMS_SUCCESS )
    {
        return -1;
    }
	if( LIS3DH_SetHPFCutOFF(LIS3DH_HPFCF_3) != MEMS_SUCCESS )
    {
        return -1;
    }
    if( LIS3DH_SetInt1Threshold(   GSEN_ACCEL_INT_THRESHOLD_RUN ) != MEMS_SUCCESS )
    {   
        return -1;
    }
    if( LIS3DH_SetIntConfiguration(LIS3DH_INT1_ZHIE_ENABLE | LIS3DH_INT1_ZLIE_ENABLE |
    								  LIS3DH_INT1_YHIE_ENABLE | LIS3DH_INT1_YLIE_ENABLE |
									  LIS3DH_INT1_XHIE_ENABLE | LIS3DH_INT1_XLIE_ENABLE ) != MEMS_SUCCESS )
    {
        return -1;
    }
    if( LIS3DH_SetIntMode(LIS3DH_INT_MODE_6D_POSITION) != MEMS_SUCCESS )
    {
       return -1;
    }
    if( LIS3DH_SetInt6D4DConfiguration(LIS3DH_INT1_6D_ENABLE) != MEMS_SUCCESS )
    {
        return -1;
    }
    if( LIS3DH_SetInt1Duration(1) != MEMS_SUCCESS )
    {
        return -1;
    }
    if( LIS3DH_SetWaterMark( 31 )  != MEMS_SUCCESS )
    {
       return -1;
    }
    if( LIS3DH_FIFOModeEnable(  LIS3DH_FIFO_STREAM_MODE ) != MEMS_SUCCESS )
    {
        return -1;
    }
#if 0
    if( LIS3DH_SetFilterDataSel(MEMS_DISABLE) != MEMS_SUCCESS )
    {
        return -1;
    }
#endif
    if( LIS3DH_SetInt1Pin(LIS3DH_I1_INT1_ON_PIN_INT1_ENABLE) != MEMS_SUCCESS )
    {
        return -1;
    }
    if( LIS3DH_HPFAOI1Enable(MEMS_ENABLE) != MEMS_SUCCESS )
    {
        return -1;
    }
    return 0;
#endif
#if( GSEN_BMI160 == 1 ) 
	gsen_cfg.id = BMI160_I2C_ADDR;
	gsen_cfg.interface = BMI160_I2C_INTF;
	gsen_cfg.read = gsen_i2c_read;
	gsen_cfg.write = gsen_i2c_write;
	gsen_cfg.delay_ms = gsen_delay_ms;
    if( bmi160_init(&gsen_cfg) != BMI160_OK )
    {
        return -1;
    }
    if( gsen_bmi160_config_work_mode() != BMI160_OK ) 
    {
        return -1;
    }
    if( gsen_config_interrupt( GSEN_ACCEL_INT_THRESHOLD_RUN ) != BMI160_OK )
    {
        return -1;
    }
    return 0;
#endif
}

/** ****************************************************************************
* @remarks       void gsen_create_task( unsigned portBASE_TYPE uxPriority )
* @brief         创建GSEN基础数据处理任务
* @param[in]     uxPriority 优先级
* @param[out]    无
* @return        无
* @attention     在系统启动后进行创建
*******************************************************************************/
void gsen_create_task( unsigned portBASE_TYPE uxPriority )
{
    gsen_state_fun = NULL;
    gsen_work_st = GSEN_WORK_ST_INIT;
    gsen_shock_time = 0;
    memset((uint8_t *)&gsen_bmi160_info , 0x00, sizeof(gsen_bmi160_info_t));
#if( GSEN_LIS3DH == 1 ) 
    memset((uint8_t *)&gsen_bmi160_p , 0x00, sizeof(gsen_bmi160_posture_t));
#endif
#if( GSEN_BMI160 == 1 ) 
    memset((uint8_t *)&gsen_bmi160_p , 0x00, sizeof(gsen_bmi160_posture_t));
    gsen_bmi160_p.quat.q0 = 1.0f;
    gsen_bmi160_p.quat.q1 = 0.0f;
    gsen_bmi160_p.quat.q2 = 0.0f;
    gsen_bmi160_p.quat.q3 = 0.0f;
#endif    
    gsen_body_p.cache.pitch = ( float * )gaoz_fibo_malloc( GSEN_CACHE_DATA_LEN * sizeof( float ) + 8 );
    if( gsen_body_p.cache.pitch == NULL )
    {
        while(1); /*创建失败*/
    }
    memset( (uint8_t *)gsen_body_p.cache.pitch , 0x00, GSEN_CACHE_DATA_LEN * sizeof( float ) );
    gsen_body_p.cache.roll = ( float * )gaoz_fibo_malloc( GSEN_CACHE_DATA_LEN * sizeof( float ) + 8 );
    if( gsen_body_p.cache.roll == NULL )
    {
        while(1); /*创建失败*/
    }
    memset( (uint8_t *)gsen_body_p.cache.roll , 0x00, GSEN_CACHE_DATA_LEN * sizeof( float ) );
    gsen_body_p.cache.yaw = ( float * )gaoz_fibo_malloc( GSEN_CACHE_DATA_LEN * sizeof( float ) + 8 );
    if( gsen_body_p.cache.yaw == NULL )
    {
        while(1); /*创建失败*/
    }
    memset( (uint8_t *)gsen_body_p.cache.yaw , 0x00, GSEN_CACHE_DATA_LEN * sizeof( float ) );
    gsen_body_p.num = 0;
    
    gens_interrupt_semaphore_create();
    /*创建任务互斥体*/
    gsen_get_data_mutex = gaoz_mutex_create();
    if( gsen_get_data_mutex == 0 )
    {
        while(1); /*创建失败*/
    }
    gsen_port_config_interrupt();
    gsen_iic_dev_init();
    gaoz_thread_create( gsen_data_handle_task, GSEN_MAN_NAME, 1024*4, NULL, uxPriority );
    
}

#if( GSEN_BMI160 == 1 ) 
/** ****************************************************************************
* @remarks       static void gsen_bmi160_accel_value_conversion( struct bmi160_sensor_data accel )
* @brief         加速度数值换算
* @param[in]     accel 加速度数据
* @param[out]    无
* @return        无
* @attention     无
*******************************************************************************/
static void gsen_bmi160_accel_value_conversion( struct bmi160_sensor_data accel )
{
    #define ACCEL_SWITCH_UNIT(x)  ((float)( x * 0.24414f))// Sensivity S2g ex:(x*9.8)/(0x8000/8)
    //#define GYRO_SWITCH_UNIT(x)   ((float)x/16.4)   		// RFS2000 ex: (x*2000)/0x7fff

    //if( accel.x>0x7fff  )
    ///{
    //    accel.x = -( 0xffff - accel.x );
    //}
    gsen_bmi160_info.accel.x = ACCEL_SWITCH_UNIT( accel.x ); //8000/32768
    
    //if( accel.y>0x7fff )
    //{
    //    accel.y = -(0xffff-accel.y);
    //}
    gsen_bmi160_info.accel.y = ACCEL_SWITCH_UNIT( accel.y );
    
    //if(accel.z>0x7fff)
    //{
    //   accel.z = -(0xffff-accel.z);
    //}
    gsen_bmi160_info.accel.z = ACCEL_SWITCH_UNIT( accel.z ); 

}
#if 0
/** ****************************************************************************
* @remarks       static void gsen_bmi160_accel_value_conversion( struct bmi160_sensor_data accel )
* @brief         角速度值换算
* @param[in]     gyro 陀螺仪数据
* @param[out]    无
* @return        无
* @attention     无
*******************************************************************************/
static void gsen_bmi160_gyro_value_conversion( struct bmi160_sensor_data gyro )
{
    if(gyro.x>0x7fff)
    {
        gyro.x = -(0xffff-gyro.x);
    }
    gsen_bmi160_info.gyro.x = (float)( ( gyro.x * 2000.0f ) / 0x7fff );
    
    if(gyro.y > 0x7fff)
    {
        gyro.y = -(0xffff-gyro.y);
    }	
    gsen_bmi160_info.gyro.y = (float)( ( gyro.y * 2000.0f ) / 0x7fff );
    
    if(gyro.z>0x7fff)
    {
        gyro.z = -(0xffff-gyro.z);
    }	
    gsen_bmi160_info.gyro.z = (float)( ( gyro.z * 2000.0f ) /0x7fff);

}
#endif
/** ****************************************************************************
* @remarks       void gsen_bmi160_sensor_euler_angle( gsen_bmi160_info_t msg )
* @brief         计算加速度传感器的欧拉角数据
* @param[in]     msg 加速度数值
* @param[out]    无
* @return        无
* @attention     无
*******************************************************************************/
static void gsen_bmi160_sensor_euler_angle( gsen_bmi160_info_t msg )
{
#if 0
    float halft = 0.0f;
    float norm;
    float vx, vy, vz;
    float ex, ey, ez;
    
    float q0q0 = gsen_bmi160_p.quat.q0 * gsen_bmi160_p.quat.q0;
    float q0q1 = gsen_bmi160_p.quat.q0 * gsen_bmi160_p.quat.q1;
    float q0q2 = gsen_bmi160_p.quat.q0 * gsen_bmi160_p.quat.q2;
    float q1q1 = gsen_bmi160_p.quat.q1 * gsen_bmi160_p.quat.q1;
    float q1q3 = gsen_bmi160_p.quat.q1 * gsen_bmi160_p.quat.q3;
    float q2q2 = gsen_bmi160_p.quat.q2 * gsen_bmi160_p.quat.q2;
    float q2q3 = gsen_bmi160_p.quat.q2 * gsen_bmi160_p.quat.q3;
    float q3q3 = gsen_bmi160_p.quat.q3 * gsen_bmi160_p.quat.q3;
    
    if( msg.accel.x * msg.accel.y * msg.accel.z == 0 )
    {
        return;
    }
    /* 获取周期时间*/
    halft = gsen_bmi160_sensor_time_period();
    
    /*1、对加速度数据进行归一化 */
    norm = sqrt(msg.accel.x*msg.accel.x + msg.accel.y*msg.accel.y + msg.accel.z*msg.accel.z); 
    msg.accel.x = msg.accel.x / norm; 
    msg.accel.y = msg.accel.y / norm; 
    msg.accel.z = msg.accel.z / norm; 
    
    /*2、DCM矩阵旋转 */
    vx = 2*(q1q3 - q0q2); 
    vy = 2*(q0q1 + q2q3); 
    vz = q0q0 - q1q1 - q2q2 + q3q3 ;
    
    /*3、在机体坐标系下做向量叉积得到补偿数据 */
    ex = msg.accel.y*vz - msg.accel.z*vy ;
    ey = msg.accel.z*vx - msg.accel.x*vz ;
    ez = msg.accel.x*vy - msg.accel.y*vx ;
    
    /*4、对误差进行PI计算，补偿角速度 */
    gsen_bmi160_p.error.x = gsen_bmi160_p.error.x + ex * GSEN_BMI160_KI;
    gsen_bmi160_p.error.y = gsen_bmi160_p.error.y + ey * GSEN_BMI160_KI;
    gsen_bmi160_p.error.z = gsen_bmi160_p.error.z + ez * GSEN_BMI160_KI;
    msg.gyro.x = msg.gyro.x + GSEN_BMI160_KP*ex + gsen_bmi160_p.error.x;
    msg.gyro.y = msg.gyro.y + GSEN_BMI160_KP*ey + gsen_bmi160_p.error.y;
    msg.gyro.z = msg.gyro.z + GSEN_BMI160_KP*ez + gsen_bmi160_p.error.z;
    
    /*5、按照四元数微分公式进行四元数更新 */
    gsen_bmi160_p.quat.q0 = gsen_bmi160_p.quat.q0 + (-gsen_bmi160_p.quat.q1*msg.gyro.x - gsen_bmi160_p.quat.q2*msg.gyro.y - gsen_bmi160_p.quat.q3*msg.gyro.z)*halft;
    gsen_bmi160_p.quat.q1 = gsen_bmi160_p.quat.q1 + (gsen_bmi160_p.quat.q0*msg.gyro.x + gsen_bmi160_p.quat.q2*msg.gyro.z - gsen_bmi160_p.quat.q3*msg.gyro.y)*halft;
    gsen_bmi160_p.quat.q2 = gsen_bmi160_p.quat.q2 + (gsen_bmi160_p.quat.q0*msg.gyro.y - gsen_bmi160_p.quat.q1*msg.gyro.z + gsen_bmi160_p.quat.q3*msg.gyro.x)*halft;
    gsen_bmi160_p.quat.q3 = gsen_bmi160_p.quat.q3 + (gsen_bmi160_p.quat.q0*msg.gyro.z + gsen_bmi160_p.quat.q1*msg.gyro.y - gsen_bmi160_p.quat.q2*msg.gyro.x)*halft;

    /*6、四元数规范化 */
    norm = sqrtf(gsen_bmi160_p.quat.q0*gsen_bmi160_p.quat.q0 + gsen_bmi160_p.quat.q1*gsen_bmi160_p.quat.q1 + gsen_bmi160_p.quat.q2*gsen_bmi160_p.quat.q2 + gsen_bmi160_p.quat.q3*gsen_bmi160_p.quat.q3);
    gsen_bmi160_p.quat.q0 = gsen_bmi160_p.quat.q0/norm;
    gsen_bmi160_p.quat.q1 = gsen_bmi160_p.quat.q1/norm;
    gsen_bmi160_p.quat.q2 = gsen_bmi160_p.quat.q2/norm;
    gsen_bmi160_p.quat.q3 = gsen_bmi160_p.quat.q3/norm;
    
    /*7、计算欧拉角  */
    /*Pitch（俯仰）：欧拉角向量的x轴  范围在-90 ~  90 
      Yaw（偏航）：欧拉角向量的y轴  范围在-180 ~  180  (由于地球自转的原因 导致数据会发生变化，需要用地磁进行补偿)
      Roll（翻滚）： 欧拉角向量的z轴 范围在-180 ~  180 */ 
    gsen_bmi160_p.euler.pitch =  asinf(2*gsen_bmi160_p.quat.q1*gsen_bmi160_p.quat.q3 - 2*gsen_bmi160_p.quat.q0*gsen_bmi160_p.quat.q2)*57.3;                        
    gsen_bmi160_p.euler.roll =  atan2f(2*gsen_bmi160_p.quat.q2*gsen_bmi160_p.quat.q3 + 2*gsen_bmi160_p.quat.q0*gsen_bmi160_p.quat.q1, -2*gsen_bmi160_p.quat.q1*gsen_bmi160_p.quat.q1 - 2*gsen_bmi160_p.quat.q2*gsen_bmi160_p.quat.q2 + 1)*57.3;
    gsen_bmi160_p.euler.yaw =  -atan2f(2*gsen_bmi160_p.quat.q1*gsen_bmi160_p.quat.q2 + 2*gsen_bmi160_p.quat.q0*gsen_bmi160_p.quat.q3, -2*gsen_bmi160_p.quat.q2*gsen_bmi160_p.quat.q2 -2*gsen_bmi160_p.quat.q3*gsen_bmi160_p.quat.q3 + 1)*57.3;  
#else

    float mid = 0.0f , s = 0.0f;
    s = (float)(msg.accel.y *  msg.accel.y + msg.accel.z *  msg.accel.z); 
    mid = (float)( msg.accel.x / sqrt(s) );  
    gsen_bmi160_p.euler.pitch = atan(mid) * 180.0/3.1416;
    
    s = (float)(msg.accel.x *  msg.accel.x + msg.accel.z *  msg.accel.z); 
    mid = (float)( msg.accel.y / sqrt(s) );
    gsen_bmi160_p.euler.roll = atan(mid) * 180.0/3.1416;  
    
    s = (float)(msg.accel.x * msg.accel.x + msg.accel.y * msg.accel.y); 
    mid = (float)(msg.accel.z / sqrt(s));
    gsen_bmi160_p.euler.yaw = atan(mid) * 180.0/3.1416; 
    
#endif
}

/** ****************************************************************************
* @remarks       static void gsen_bmi160_sensor_body_posture( gsen_bmi160_info_t msg )
* @brief         获取车身的姿态
* @param[in]     euler 获取车身的欧拉角
* @param[out]    无
* @return        无
* @attention     无
*******************************************************************************/
static void gsen_bmi160_sensor_body_posture( gsen_bmi160_posture_t euler )
{
    float res_pitch = 0.0f, res_roll = 0.0f, res_yaw = 0.0f;
    uint32_t i = 0;
    static uint8_t gsen_silent_time = 0;
#if 0
    log_debug_printf(INFOR_MODE,GSEN_MAN_NAME ,"bmi160 euler angle:pitch=%f,roll=%f,yaw=%f\r\n",euler.euler.pitch,euler.euler.roll,euler.euler.yaw);
#endif
    gsen_body_p.cache.pitch[gsen_body_p.num] = euler.euler.pitch;
    gsen_body_p.cache.roll[gsen_body_p.num]= euler.euler.roll;
    gsen_body_p.cache.yaw[gsen_body_p.num] = euler.euler.yaw;
    gsen_body_p.num ++;
    if( gsen_body_p.num >= GSEN_CACHE_DATA_LEN   )
    {
        gsen_body_p.num = 0;
        for( i = 1 ; i < GSEN_CACHE_DATA_LEN ; i ++ )
        {
            res_pitch += gsen_body_p.cache.pitch[i-1] - gsen_body_p.cache.pitch[i];
            res_roll += gsen_body_p.cache.roll[i-1] - gsen_body_p.cache.roll[i];
            res_yaw += gsen_body_p.cache.yaw[i-1] - gsen_body_p.cache.yaw[i];
        }
        if( res_pitch < 0 )
        {
            res_pitch = ( 0 - res_pitch );
        }
        if( res_roll < 0 )
        {
            res_roll = ( 0 - res_roll );
        }
        if( res_yaw < 0 )
        {
            res_yaw = ( 0 - res_yaw );
        }
        if(( res_pitch > 45.0f ) || ( res_roll > 45.0f ) || ( res_yaw > 45.0f ))
        {
            gsen_sensor_body_st_set( GSEN_ATT_ST_ROLLOVER );
            gsen_sensor_state_cb( GSEN_ATT_ST_ROLLOVER );
            log_debug_send( INFOR_MODE,GSEN_MAN_NAME ,"GSEN detect equipment vibration:GSEN_ATT_ST_ROLLOVER\r\n",0 );
        }
#if 0        
        log_debug_printf(INFOR_MODE,GSEN_MAN_NAME ,"bmi160 euler angle:res_pitch=%f,res_roll=%f,res_yaw=%f\r\n",res_pitch,res_roll,res_yaw);
#endif
        arm_var_f32( (float *)gsen_body_p.cache.pitch , GSEN_CACHE_DATA_LEN , (float *)&res_pitch );
        arm_var_f32( (float *)gsen_body_p.cache.roll , GSEN_CACHE_DATA_LEN , (float *)&res_roll );
        arm_var_f32( (float *)gsen_body_p.cache.yaw , GSEN_CACHE_DATA_LEN , (float *)&res_yaw );
#if 0
        log_debug_printf(INFOR_MODE,GSEN_MAN_NAME ,"bmi160 euler angle:res_pitch=%f,res_roll=%f,res_yaw=%f\r\n",res_pitch,res_roll,res_yaw);
#endif
        /*判断车辆是否发生震动*/
        if(( res_pitch > 10.0f ) || ( res_roll > 10.0f ) || ( res_yaw > 10.0f ))
        {   
            gsen_shock_time++;
            gsen_silent_time = 0;
            if( gsen_shock_time >= GSEN_SHOCK_MAX_TIME )        /*连续运动30秒认为车辆处于运动状态*/
            {   
                gsen_shock_time = GSEN_SHOCK_MAX_TIME;
                gsen_sensor_body_st_set( GSEN_ATT_ST_SHIFTING );
                gsen_sensor_state_cb( GSEN_ATT_ST_SHIFTING );
                log_debug_send( INFOR_MODE,GSEN_MAN_NAME ,"GSEN detect equipment vibration:GSEN_ATT_ST_SHIFTING\r\n",0 );
            }
            else
            {
                gsen_sensor_body_st_set( GSEN_ATT_ST_SHAKE );
                gsen_sensor_state_cb( GSEN_ATT_ST_SHAKE );     /*车辆产生振动*/
                log_debug_send( INFOR_MODE,GSEN_MAN_NAME ,"GSEN detect equipment vibration:GSEN_ATT_ST_SHAKE\r\n",0 );
            }
        }
        else
        {
            gsen_silent_time ++;
            gsen_shock_time = 0;
            if( gsen_silent_time >= GSEN_SILENT_MAX_TIME )
            {
                gsen_silent_time = GSEN_SILENT_MAX_TIME;
                gsen_sensor_body_st_set( GSEN_ATT_ST_INIT );
                gsen_sensor_state_cb( GSEN_ATT_ST_INIT );
            }
        }
    }
}


/** ****************************************************************************
* @remarks       static void gsen_bmi160_sensor_body_crash( gsen_bmi160_body_posture_t crash )
* @brief         分析车身碰撞情况
* @param[in]     crash 加速度传感器碰撞后的FIFO中的数据
* @param[out]    无
* @return        无
* @attention     无
*******************************************************************************/
static void gsen_bmi160_sensor_body_crash( gsen_bmi160_body_posture_t *crash )
{
    float max_pitch = 0.0f, max_roll = 0.0f, max_yaw = 0.0f;
    float min_pitch = 0.0f, min_roll = 0.0f, min_yaw = 0.0f;
    uint32_t pos_pitch = 0,pos_roll = 0,pos_yaw = 0;
    uint32_t pos_num = 0 , i = 0;
    
    if( crash->num != 0 )
    {
        pos_num = crash->num;
        arm_max_f32((float32_t * )crash->cache.pitch, crash->num,(float32_t * )&max_pitch, (uint32_t * )&pos_pitch );
        arm_max_f32((float32_t * )crash->cache.roll, crash->num,(float32_t * )&max_roll, (uint32_t * )&pos_roll );
        arm_max_f32((float32_t * )crash->cache.yaw, crash->num,(float32_t * )&max_yaw, (uint32_t * )&pos_yaw );
        
        //log_debug_printf(INFOR_MODE,GSEN_MAN_NAME ,"arm_max_f32:x=%f,y=%f,z=%f\r\n",max_pitch,max_roll,max_yaw  );
        if(( max_pitch > 5000.0f ) || ( max_roll > 5000.0f ) || ( max_yaw > 5000.0f ))
        {   
            gsen_sensor_body_st_set( GSEN_ATT_ST_CRASH );
            gsen_sensor_state_cb( GSEN_ATT_ST_CRASH );
            log_debug_send( INFOR_MODE,GSEN_MAN_NAME ,"GSEN detect equipment vibration:GSEN_ATT_ST_CRASH\r\n",0 );
        }
        
        arm_min_f32((float32_t * )crash->cache.pitch, crash->num,(float32_t * )&min_pitch, (uint32_t * )&pos_pitch );
        arm_min_f32((float32_t * )crash->cache.roll, crash->num,(float32_t * )&min_roll, (uint32_t * )&pos_roll );
        arm_min_f32((float32_t * )crash->cache.yaw, crash->num,(float32_t * )&min_yaw, (uint32_t * )&pos_yaw );
        
        //log_debug_printf(INFOR_MODE,GSEN_MAN_NAME ,"arm_min_f32:x=%f,y=%f,z=%f\r\n",min_pitch,min_roll,min_yaw  );
        if(( min_pitch < -5000.0f ) || ( min_roll < -5000.0f ) || ( min_yaw < -5000.0f ))
        {
            gsen_sensor_body_st_set( GSEN_ATT_ST_CRASH );
            gsen_sensor_state_cb( GSEN_ATT_ST_CRASH );
            log_debug_send( INFOR_MODE,GSEN_MAN_NAME ,"GSEN detect equipment vibration:GSEN_ATT_ST_CRASH\r\n",0 );
        }
        crash->num = 0;
        if( gsen_sensor_body_st_get() != GSEN_ATT_ST_CRASH )
        {
            for( i = 0 ; i < pos_num ; i ++ )
            {
                gsen_bmi160_info.accel.x = crash->cache.pitch[i];
                gsen_bmi160_info.accel.y = crash->cache.roll[i];
                gsen_bmi160_info.accel.z = crash->cache.yaw[i];
                gsen_bmi160_sensor_euler_angle( gsen_bmi160_info );     /*计算欧拉角*/
                gsen_bmi160_sensor_body_posture( gsen_bmi160_p );       /*判断车身的状态*/
            }
        }
    }
}

#endif

#if( GSEN_LIS3DH == 1 ) 
/** ****************************************************************************
* @remarks       static void gsen_lis3dh_accraw_get( int16_t* pdata )
* @brief         获取3轴加速度传感器寄存器数据
* @param[in]     无
* @param[out]    *pdata 数据目标地址
* @return        pdPASS 成功      pdFAIL 失败
* @attention     无
*******************************************************************************/
static uint32_t gsen_lis3dh_accraw_get( int16_t *pdata )
{
    uint8_t i = 0;
    uint8_t reg_value[6] = { 0 };
    int16_t symbol[3] = { 0 };
    uint32_t ret = pdPASS;
#if 0
    for( i = 0; i < 6; i++ )
    {
        if( LIS3DH_ReadReg( LIS3DH_OUT_X_L+i, reg_value+i ) == 0 ) 
        {
            ret = pdFAIL;
            break;
        }
    }
#else
    /*为了增加系统读取数据的稳定性，连续读取多个字节*/
    if( gaoz_i2c_get_data( LIS3DH_MEMS_I2C_ADDRESS , LIS3DH_OUT_X_L|0x80, false, reg_value, 6 ) != 0 )
    {
        ret = pdFAIL;
    }
#endif
    if( ret == pdPASS )
    {
        /* Format the data. */
        for( i = 0; i < 3; i++ )
        {
            if( reg_value[2*i+1] & 0x80 ) 
            {
                symbol[i] = 0xF000;
            }
            else 
            {
                symbol[i] = 0x0000;  
            }
        }
        pdata[0] =symbol[0]  | (( ( ( ( int16_t )reg_value[1] ) << 8 ) + ( int16_t )reg_value[0] ) >> 4);
        pdata[1] =symbol[1]  | ((( ( ( int16_t )reg_value[3] ) << 8 ) + ( int16_t )reg_value[2] ) >> 4);
        pdata[2] =symbol[2]  | ((( ( ( int16_t )reg_value[5] ) << 8 ) + ( int16_t )reg_value[4] ) >> 4);
    }
    return ret;
}

/** ****************************************************************************
* @remarks       static uint32_t gsen_lis3dh_sensitivity_get( uint8_t* sensitivity )
* @brief         获取加速度传感器的加速度值
* @param[in]     无
* @param[out]    *sensitivity 数据目标地址
* @return        pdPASS 成功      pdFAIL 失败
* @attention     无
*******************************************************************************/
static uint32_t gsen_lis3dh_sensitivity_get( uint8_t *sensitivity )
{
    static uint8_t read_state = 0;
    static uint8_t fullscale = 0;	
    uint32_t ret = pdFAIL;

    if( read_state == 0 )
    {
        if( LIS3DH_ReadReg( LIS3DH_CTRL_REG4,&fullscale ) == 1 )
        { 
            read_state = 1;
            fullscale = ( fullscale & 0x30 ) >> 4;
        }
    }
    else
    {
        switch (fullscale)
        {
            case 0:
                *sensitivity = 1;
                ret = pdPASS;
            break;
            case 1:
                *sensitivity = 2;
                ret = pdPASS;
            break;
            case 2:
                *sensitivity = 4;
                ret = pdPASS;
            break;
            case 3:
                *sensitivity = 12;
                ret = pdPASS;
            break;
            default : 
                ret = pdFAIL;
            break;
        }
    }
    return ret;
}

/** ****************************************************************************
* @remarks       static uint32_t gsen_lis3dh_accvalue_get( AxesRaw_t *pdata )
* @brief         获取加速度传感器的加速度值
* @param[in]     无
* @param[out]    *pdata 数据目标地址
* @return        pdPASS 成功      pdFAIL 失败
* @attention     无
*******************************************************************************/
static uint32_t gsen_lis3dh_accvalue_get( AxesRaw_t *pdata )
{
	int16_t data_raw[3] = { 0 };
	uint8_t sensitivity = 0;
    uint32_t ret = pdFAIL;
	if( gsen_lis3dh_accraw_get( data_raw ) == pdPASS )
    {
    	if( gsen_lis3dh_sensitivity_get( &sensitivity ) == pdPASS )
        {   
            pdata->AXIS_X  =  ( int32_t )( data_raw[0] * sensitivity );
            pdata->AXIS_Y  =  ( int32_t )( data_raw[1] * sensitivity );
            pdata->AXIS_Z  =  ( int32_t )( data_raw[2] * sensitivity );
            ret = pdPASS;
        }
   }
    return ret;
}

/** ****************************************************************************
* @remarks       static void gsen_lis3dh_accvalue_conversion( AxesRaw_t pdata )
* @brief         更新3轴加速度传感器
* @param[in]     pdata 三轴加速度数据
* @param[out]    无
* @return        无
* @attention     无
*******************************************************************************/
static void gsen_lis3dh_accvalue_conversion( AxesRaw_t pdata )
{    
    gsen_bmi160_info.accel.x = (float)pdata.AXIS_X;
    gsen_bmi160_info.accel.y = (float)pdata.AXIS_Y;
    gsen_bmi160_info.accel.z = (float)pdata.AXIS_Z  ;
}


/** ****************************************************************************
* @remarks       void gsen_lis3dh_data_get(gsen_bmi160_info_t *gsen_data)
* @brief         获取三轴最新数据
* @param[in]     pdata 三轴加速度数据存储位置
* @param[out]    无
* @return        无
* @attention     无
*******************************************************************************/
void gsen_lis3dh_data_get(gsen_bmi160_info_t *gsen_data)
{
    gsen_data->accel.x = gsen_bmi160_info.accel.x;
    gsen_data->accel.y = gsen_bmi160_info.accel.y;
    gsen_data->accel.z = gsen_bmi160_info.accel.z;
}

/** ****************************************************************************
* @remarks       static void gsen_lis3dh_sensor_body_posture( gsen_bmi160_posture_t euler )
* @brief         分析车身震动情况
* @param[in]     msg 加速度传感器数据
* @param[out]    无
* @return        无
* @attention     无
*******************************************************************************/
static void gsen_lis3dh_sensor_body_posture( gsen_bmi160_posture_t euler )
{
    float res_pitch = 0.0f, res_roll = 0.0f, res_yaw = 0.0f;
    uint32_t i = 0;
    static uint8_t gsen_silent_time = 0;
#if 0
    log_debug_printf(INFOR_MODE,GSEN_MAN_NAME ,"Lis3dh euler angle:pitch=%f,roll=%f,yaw=%f\r\n",euler.euler.pitch,euler.euler.roll,euler.euler.yaw);
#endif
    gsen_body_p.cache.pitch[gsen_body_p.num] = euler.euler.pitch;
    gsen_body_p.cache.roll[gsen_body_p.num]= euler.euler.roll;
    gsen_body_p.cache.yaw[gsen_body_p.num] = euler.euler.yaw;
    gsen_body_p.num ++;
    if( gsen_body_p.num >= GSEN_CACHE_DATA_LEN   )
    {
        gsen_body_p.num = 0;
        for( i = 1 ; i < GSEN_CACHE_DATA_LEN ; i ++ )
        {
            res_pitch += gsen_body_p.cache.pitch[i-1] - gsen_body_p.cache.pitch[i];
            res_roll += gsen_body_p.cache.roll[i-1] - gsen_body_p.cache.roll[i];
            res_yaw += gsen_body_p.cache.yaw[i-1] - gsen_body_p.cache.yaw[i];
        }
        if( res_pitch < 0 )
        {
            res_pitch = ( 0 - res_pitch );
        }
        if( res_roll < 0 )
        {
            res_roll = ( 0 - res_roll );
        }
        if( res_yaw < 0 )
        {
            res_yaw = ( 0 - res_yaw );
        }
        if(( res_pitch > 45.0f ) || ( res_roll > 45.0f ) || ( res_yaw > 45.0f ))
        {
            gsen_sensor_body_st_set( GSEN_ATT_ST_ROLLOVER );
            gsen_sensor_state_cb( GSEN_ATT_ST_ROLLOVER );
            log_debug_send( INFOR_MODE,GSEN_MAN_NAME ,"GSEN detect equipment vibration:GSEN_ATT_ST_ROLLOVER\r\n",0 );
        }
#if 0        
        log_debug_printf(INFOR_MODE,GSEN_MAN_NAME ,"Lis3dh euler angle:res_pitch=%f,res_roll=%f,res_yaw=%f\r\n",res_pitch,res_roll,res_yaw);
#endif
        arm_var_f32( (float *)gsen_body_p.cache.pitch , GSEN_CACHE_DATA_LEN , (float *)&res_pitch );
        arm_var_f32( (float *)gsen_body_p.cache.roll , GSEN_CACHE_DATA_LEN , (float *)&res_roll );
        arm_var_f32( (float *)gsen_body_p.cache.yaw , GSEN_CACHE_DATA_LEN , (float *)&res_yaw );
#if 0
        log_debug_printf(INFOR_MODE,GSEN_MAN_NAME ,"Lis3dh euler angle:res_pitch=%f,res_roll=%f,res_yaw=%f\r\n",res_pitch,res_roll,res_yaw);
#endif
        /*判断车辆是否发生震动*/
        if(( res_pitch > 50.0f ) || ( res_roll > 50.0f ) || ( res_yaw > 50.0f ))
        {   
            gsen_shock_time++;
            gsen_silent_time = 0;
            if( gsen_shock_time >= GSEN_SHOCK_MAX_TIME )        /*连续运动30秒认为车辆处于运动状态*/
            {   
                gsen_shock_time = GSEN_SHOCK_MAX_TIME;
                gsen_sensor_body_st_set( GSEN_ATT_ST_SHIFTING );
                gsen_sensor_state_cb( GSEN_ATT_ST_SHIFTING );
                log_debug_send( INFOR_MODE,GSEN_MAN_NAME ,"GSEN detect equipment vibration:GSEN_ATT_ST_SHIFTING\r\n",0 );
            }
            else
            {
                gsen_sensor_body_st_set( GSEN_ATT_ST_SHAKE );
                gsen_sensor_state_cb( GSEN_ATT_ST_SHAKE );     /*车辆产生振动*/
                log_debug_send( INFOR_MODE,GSEN_MAN_NAME ,"GSEN detect equipment vibration:GSEN_ATT_ST_SHAKE\r\n",0 );
            }
        }
        else
        {
            gsen_silent_time ++;
            gsen_shock_time = 0;
            if( gsen_silent_time >= GSEN_SILENT_MAX_TIME )
            {
                gsen_silent_time = GSEN_SILENT_MAX_TIME;
                gsen_sensor_body_st_set( GSEN_ATT_ST_INIT );
                gsen_sensor_state_cb( GSEN_ATT_ST_INIT );
            }
        }
    }
}

/** ****************************************************************************
* @remarks       static void gsen_lis3dh_sensor_body_crash( gsen_bmi160_body_posture_t crash )
* @brief         分析车身碰撞情况
* @param[in]     crash 加速度传感器碰撞后的FIFO中的数据
* @param[out]    无
* @return        无
* @attention     无
*******************************************************************************/
static void gsen_lis3dh_sensor_body_crash( gsen_bmi160_body_posture_t *crash )
{
    float max_pitch = 0.0f, max_roll = 0.0f, max_yaw = 0.0f;
    float min_pitch = 0.0f, min_roll = 0.0f, min_yaw = 0.0f;
    uint32_t pos_pitch = 0,pos_roll = 0,pos_yaw = 0;
    uint32_t pos_num = 0 , i = 0;

    if( crash->num != 0 )
    {
        pos_num = crash->num;
        arm_max_f32((float32_t * )crash->cache.pitch, crash->num,(float32_t * )&max_pitch, (uint32_t * )&pos_pitch );
        arm_max_f32((float32_t * )crash->cache.roll, crash->num,(float32_t * )&max_roll, (uint32_t * )&pos_roll );
        arm_max_f32((float32_t * )crash->cache.yaw, crash->num,(float32_t * )&max_yaw, (uint32_t * )&pos_yaw );
        if(( max_pitch > 5000.0f ) || ( max_roll > 5000.0f ) || ( max_yaw > 5000.0f ))
        {   
            gsen_sensor_body_st_set( GSEN_ATT_ST_CRASH );
            gsen_sensor_state_cb( GSEN_ATT_ST_CRASH );
            log_debug_send( INFOR_MODE,GSEN_MAN_NAME ,"GSEN detect equipment vibration:GSEN_ATT_ST_CRASH\r\n",0 );
        }
        arm_min_f32((float32_t * )crash->cache.pitch, crash->num,(float32_t * )&min_pitch, (uint32_t * )&pos_pitch );
        arm_min_f32((float32_t * )crash->cache.roll, crash->num,(float32_t * )&min_roll, (uint32_t * )&pos_roll );
        arm_min_f32((float32_t * )crash->cache.yaw, crash->num,(float32_t * )&min_yaw, (uint32_t * )&pos_yaw );
        if(( min_pitch < -5000.0f ) || ( min_roll < -5000.0f ) || ( min_yaw < -5000.0f ))
        {
            gsen_sensor_body_st_set( GSEN_ATT_ST_CRASH );
            gsen_sensor_state_cb( GSEN_ATT_ST_CRASH );
            log_debug_send( INFOR_MODE,GSEN_MAN_NAME ,"GSEN detect equipment vibration:GSEN_ATT_ST_CRASH\r\n",0 );
        }
        crash->num = 0;
        if( gsen_sensor_body_st_get() != GSEN_ATT_ST_CRASH )
        {   
            for( i = 0 ; i < pos_num ; i ++ )
            {
                gsen_bmi160_info.accel.x = crash->cache.pitch[i];
                gsen_bmi160_info.accel.y = crash->cache.roll[i];
                gsen_bmi160_info.accel.z = crash->cache.yaw[i];
                gsen_lis3dh_sensor_euler_angle( gsen_bmi160_info );
                gsen_lis3dh_sensor_body_posture( gsen_bmi160_p );       /*判断车身的状态*/
            }
        }
    }
}

/** ****************************************************************************
* @remarks       static void gsen_lis3dh_sensor_euler_angle( gsen_bmi160_info_t pdata )
* @brief         计算车身的姿态
* @param[in]     pdata 加速度数据
* @param[out]    无
* @return        无
* @attention     无
*******************************************************************************/
static void gsen_lis3dh_sensor_euler_angle( gsen_bmi160_info_t pdata )
{
#if 0
    gsen_bmi160_p.euler.pitch  = atan2((float)( 0 - pdata.accel.y),pdata.accel.z ) * 180 / 3.14159; //转换为度数
    if( gsen_bmi160_p.euler.pitch < 0 )
    {
        gsen_bmi160_p.euler.pitch = ( 0 - gsen_bmi160_p.euler.pitch );
    }
    gsen_bmi160_p.euler.roll = atan2((float)( pdata.accel.x ),pdata.accel.z ) * 180 / 3.14159;     //转换为度数
    if( gsen_bmi160_p.euler.roll < 0 )
    {
        gsen_bmi160_p.euler.roll = ( 0 - gsen_bmi160_p.euler.roll );
    }
    gsen_bmi160_p.euler.yaw = 0;
#else
    float mid = 0.0f , s = 0.0f;
    s = (float)(pdata.accel.y *  pdata.accel.y + pdata.accel.z *  pdata.accel.z); 
    mid = (float)( pdata.accel.x / sqrt(s) );  
    gsen_bmi160_p.euler.pitch = atan(mid) * 180.0/3.1416;
    
    s = (float)(pdata.accel.x *  pdata.accel.x + pdata.accel.z *  pdata.accel.z); 
    mid = (float)( pdata.accel.y / sqrt(s) );
    gsen_bmi160_p.euler.roll = atan(mid) * 180.0/3.1416;  
    
    s = (float)(pdata.accel.x * pdata.accel.x + pdata.accel.y * pdata.accel.y); 
    mid = (float)(pdata.accel.z / sqrt(s));
    gsen_bmi160_p.euler.yaw = atan(mid) * 180.0/3.1416; 
#endif
}
#endif

/** ****************************************************************************
* @remarks       static uint32_t gsen_sensor_data_get(void)
* @brief         获取加速度传感器中寄存器的数据
* @param[in]     无
* @param[out]    无
* @return        无
* @attention     无
*******************************************************************************/
static uint32_t gsen_sensor_data_get(void)
{
    uint32_t ret =pdFAIL , osR = 0;
    if( gsen_get_data_mutex != 0 )
    {   
        gaoz_mutex_lock( gsen_get_data_mutex );
        osR = 1;
    }
    
#if( GSEN_LIS3DH == 1 ) 
    AxesRaw_t data = { 0 };
    if( gsen_lis3dh_accvalue_get( &data ) == pdPASS )
    {   
        gsen_lis3dh_accvalue_conversion( data );
        ret =pdPASS;
    }
#endif

#if( GSEN_BMI160 == 1 ) 
    struct bmi160_sensor_data accel;
    //struct bmi160_sensor_data gyro;
    /*读取传感器中的数据*/
    if( bmi160_get_sensor_data( BMI160_ACCEL_ONLY, &accel, NULL, &gsen_cfg) == BMI160_OK )
    {
        gsen_bmi160_accel_value_conversion( accel );        /*换算加速度数据*/
        //gsen_bmi160_gyro_value_conversion( gyro );        /*换算角速度数据*/
        ret =pdPASS;
    }
#endif
    if( osR != 0 )
    {
        gaoz_mutex_unlock( gsen_get_data_mutex );
    }   

    return ret;
}

/** ****************************************************************************
* @remarks       uint32_t gsen_sensor_interrupt_data_get( void )
* @brief         读取加速度传感产生中断时的数据。
* @param[in]     无
* @param[out]    *info 数据输出指针
* @return        pdPASS 成功 ；pdFAIL 失败
* @attention     无
*******************************************************************************/
static uint32_t gsen_sensor_interrupt_data_get(void)
{
    uint32_t ret =pdFAIL , osR = 0;
    if( gsen_get_data_mutex != 0 )
    {   
        gaoz_mutex_lock( gsen_get_data_mutex );
        osR = 1;
    }
#if( GSEN_LIS3DH == 1 ) 
    AxesRaw_t data = { 0 };
    uint8_t fifo_num = 0 , i = 0;
    LIS3DH_GetFifoSourceFSS( &fifo_num );
    if( fifo_num > GSEN_CACHE_DATA_LEN )
    {
        fifo_num = GSEN_CACHE_DATA_LEN;
    }
    gsen_body_p.num = 0;
    for( i = 0 ; i < fifo_num ; i ++ )
    {
        if( gsen_lis3dh_accvalue_get( &data ) == pdPASS )
        {
            gsen_body_p.cache.pitch[gsen_body_p.num] = (float)data.AXIS_X;
            gsen_body_p.cache.roll[gsen_body_p.num]= (float)data.AXIS_Y;
            gsen_body_p.cache.yaw[gsen_body_p.num] = (float)data.AXIS_Z  ;
            gsen_body_p.num ++;
            ret =pdPASS;
        }
    }
    if( gsen_body_p.num < GSEN_CACHE_DATA_LEN )
    {
        for( i = gsen_body_p.num ; i < GSEN_CACHE_DATA_LEN ; i ++ )
        {
            if( gsen_lis3dh_accvalue_get( &data ) == pdPASS )
            {
                gsen_body_p.cache.pitch[gsen_body_p.num] = (float)data.AXIS_X;
                gsen_body_p.cache.roll[gsen_body_p.num]= (float)data.AXIS_Y;
                gsen_body_p.cache.yaw[gsen_body_p.num] = (float)data.AXIS_Z  ;
                gsen_body_p.num ++;
                ret =pdPASS;
            }
        }
    }
#endif

#if( GSEN_BMI160 == 1 ) 
    struct bmi160_sensor_data accel;
    uint8_t i = 0;
    /*读取传感器中的数据*/
    gsen_body_p.num = 0;
    for( i = 0 ; i < GSEN_CACHE_DATA_LEN ; i ++ )
    {
        if( bmi160_get_sensor_data( BMI160_ACCEL_ONLY, &accel, NULL, &gsen_cfg) == BMI160_OK )
        {
            gsen_bmi160_accel_value_conversion( accel );        /*换算加速度数据*/
            gsen_body_p.cache.pitch[i] = gsen_bmi160_info.accel.x ;
            gsen_body_p.cache.roll[i]= gsen_bmi160_info.accel.y ;
            gsen_body_p.cache.yaw[i] = gsen_bmi160_info.accel.z  ;
            gsen_body_p.num++;
            ret =pdPASS;
        }
    }
#endif
    if( osR != 0 )
    {
        gaoz_mutex_unlock( gsen_get_data_mutex );
    }   
    return ret;
}
/** ****************************************************************************
* @remarks       uint32_t gsen_sensor_data_read( gsen_bmi160_info_t *info )
* @brief         读取加速度传感器数据，获取当前的状态。
* @param[in]     无
* @param[out]    *info 数据输出指针
* @return        pdPASS 成功 ；pdFAIL 失败
* @attention     无
*******************************************************************************/
uint32_t gsen_sensor_data_read( gsen_bmi160_info_t *info )
{
	uint32_t ret = pdFAIL, osR = 0;
    
    if( info == NULL )
    {
        return pdFAIL;
    }
    if( gsen_get_data_mutex != 0 )
    {   
        gaoz_mutex_lock( gsen_get_data_mutex );
        osR = 1;
    }
    
    memcpy( (uint8_t *)info,(uint8_t *)&gsen_bmi160_info, sizeof( gsen_bmi160_info_t ) );
    ret = pdPASS;
    
    if(osR != 0)
    {
        gaoz_mutex_unlock( gsen_get_data_mutex );
    } 
    return ret;
}


/** ****************************************************************************
* @remarks       void gsen_sensor_state_cb_set( gsen_state_fun_t fun )
* @brief         设置GSEN状态的回调函数
* @param[in]     fun 回调函数指针
* @param[out]    无
* @return        无
* @attention     无
*******************************************************************************/
void gsen_sensor_state_cb_set( gsen_state_fun_t fun )
{
    if( gsen_state_fun == NULL )
    {
        gsen_state_fun = fun;
    }
}

/** ****************************************************************************
* @remarks       static uint32_t gsen_sensor_state_cb( unit32_t st )
* @brief         GSEN 状态 ，回调函数
* @param[in]     st GSEN 当前的状态
* @param[out]    无
* @return        pdPASS 成功  pdFAIL 失败
* @attention     无
*******************************************************************************/
static uint32_t gsen_sensor_state_cb( uint32_t st )
{
	if( gsen_state_fun == NULL )
    {
		return pdFAIL;
    }
	gsen_state_fun( st );
	return pdPASS;
}

/** ****************************************************************************
* @remarks       static  gsen_sensor_work_st_set( gsen_bmi160_work_state_e st )
* @brief         设置加速度传感器状态
* @param[in]     无
* @param[out]    无
* @return        GSEN_WORK_ST_ERR 状态失败  GSEN_WORK_ST_OK 状态成功
* @attention     无
*******************************************************************************/
static void gsen_sensor_work_st_set( gsen_bmi160_work_state_e st )
{
    gsen_work_st = st;
}

/** ****************************************************************************
* @remarks       gsen_bmi160_work_state_e gsen_sensor_work_st_get( void )
* @brief         获取加速度传感器的工作状态
* @param[in]     无
* @param[out]    无
* @return        GSEN_WORK_ST_ERR 状态失败  GSEN_WORK_ST_OK 状态成功
* @attention     无
*******************************************************************************/
gsen_bmi160_work_state_e gsen_sensor_work_st_get( void )
{
    return gsen_work_st;
}

/** ****************************************************************************
* @remarks       gsen_bmi160_body_state_e gsen_sensor_body_st_get( void )
* @brief         获取车身运动的状态
* @param[in]     无
* @param[out]    无
* @return        车身的运动状态
* @attention     无
*******************************************************************************/
gsen_bmi160_body_state_e gsen_sensor_body_st_get( void )
{
    return gsen_bmi160_body_st;
}

/** ****************************************************************************
* @remarks       void gsen_sensor_body_st_set( gsen_bmi160_body_state_e st )
* @brief         设置车身运动的状态
* @param[in]     无
* @param[out]    无
* @return        车身的运动状态
* @attention     无
*******************************************************************************/
void gsen_sensor_body_st_set( gsen_bmi160_body_state_e st )
{
    gsen_bmi160_body_st = st;
}

/** ****************************************************************************
* @remarks       static void gens_interrupt_semaphore_create(void)
* @brief         创建GSEN数据接收信号量
* @param[in]     无
* @param[out]    无
* @return        无
* @attention     无
*******************************************************************************/
static void gens_interrupt_semaphore_create(void)
{
    gsen_interrupe_semaphore = gaoz_sem_new(0);
    if( gsen_interrupe_semaphore == 0 )
    {
        while( 1 ); /*创建失败*/
    }
}

/** ****************************************************************************
* @remarks       static uint8_t ble_drive_data_semaphore_get(void)
* @brief         获取GSEN底层接收数据的信号量
* @param[in]     无
* @param[out]    无
* @return        1 接收到数据 ， 0 未接收到数据，超时
* @attention     阻塞状态 时间3s GSEN_INTER_SAM_REV_WAIT_TIME
*******************************************************************************/
static uint8_t gens_interrupt_semaphore_get(void)
{
    if( gsen_interrupe_semaphore == 0 )
    {
        gens_interrupt_semaphore_create();
    }
    if( gaoz_sem_try_wait( gsen_interrupe_semaphore, GSEN_INTER_SAM_REV_WAIT_TIME ) == true ) //等待回应信号量
    {
        return 1;
    }
    else
    {
        return 0;
    }
}

/** ****************************************************************************
* @remarks       static uint8_t ble_drive_data_semaphore_set(void)
* @brief         发送 GSEN底层接收数据的信号量
* @param[in]     无
* @param[out]    无
* @return       无
* @attention     无
*******************************************************************************/
static void gens_interrupt_semaphore_set(void)
{
    if( gsen_interrupe_semaphore != 0 )
    {
        gaoz_sem_signal( gsen_interrupe_semaphore ); /*发送信号量，通知任务*/
    }
}

/** ****************************************************************************
* @remarks       static uint32_t gsen_task_into_run_mode(void)
* @brief         GSEN任务进入运行状态
* @param[in]     无
* @param[out]    无
* @return        pdPASS 设置成功 pdFAIL 设置失败
* @attention     无
*******************************************************************************/
static uint32_t gsen_task_into_run_mode(void)
{
    task_modules_e state = TASK_MODE_INIT;
    uint32_t ret = pdFAIL;
    state = power_man_save_state_get( gsen_power_man_id );  /*获取任务的运行状态*/
    if( state != TASK_RUN_MODE )
    {   
        gsen_shock_time = 0;
        gsen_sensor_body_st_set( GSEN_ATT_ST_INIT );
        gsen_config_interrupt( GSEN_ACCEL_INT_THRESHOLD_RUN );/*关闭GSEN外部中断*/
        power_man_save_state_set( gsen_power_man_id ,TASK_RUN_MODE );
        log_debug_send(INFOR_MODE,GSEN_MAN_NAME ,"<<<<<<<<<<*****GSEN task enters RUN mode*****>>>>>>>>>>\r\n", 0);
        ret = pdPASS;
    }
    else if( state == TASK_RUN_MODE )
    {
        ret = pdPASS;
    }
    return ret;
}
/** ****************************************************************************
* @remarks       static uint32_t gsen_task_into_sleep_mode(void)
* @brief         GSEN 任务进入睡眠状态
* @param[in]     无
* @param[out]    无
* @return        pdPASS 设置成功 pdFAIL 设置失败
* @attention     无
*******************************************************************************/
static uint32_t gsen_task_into_sleep_mode(void)
{
    task_modules_e state = TASK_MODE_INIT;
    uint32_t ret = pdFAIL; 
    state = power_man_save_state_get( gsen_power_man_id ); /*获取任务的运行状态*/
    if( state != TASK_SLEEP_MODE )
    {   
        gsen_shock_time = 0;
        gsen_sensor_body_st_set( GSEN_ATT_ST_INIT );
        gsen_config_interrupt( GSEN_ACCEL_INT_THRESHOLD_SLEEP ); /*使能GSEN外部中断*/
        power_man_save_state_set( gsen_power_man_id ,TASK_SLEEP_MODE );
        log_debug_send(INFOR_MODE,GSEN_MAN_NAME ,"<<<<<<<<<<*****GSEN task enters SLEEP mode*****>>>>>>>>>>\r\n", 0);
        ret = pdPASS;
    }
    else if( state == TASK_SLEEP_MODE )
    {
        ret = pdPASS;
    }
    return ret;
}

/** ****************************************************************************
* @remarks       static uint32_t gsen_task_into_stop_mode(void)
* @brief         GSEN 任务进入停止状态
* @param[in]     无
* @param[out]    无
* @return        pdPASS 设置成功 pdFAIL 设置失败
* @attention     无
*******************************************************************************/
static uint32_t gsen_task_into_stop_mode(void)
{
    task_modules_e state = TASK_MODE_INIT;
    uint32_t ret = pdFAIL; 
    state = power_man_save_state_get( gsen_power_man_id );  /*获取任务的运行状态*/
    if( state != TASK_STOP_MODE )
    {   
        gsen_shock_time = 0;
        gsen_sensor_body_st_set( GSEN_ATT_ST_INIT );
        gsen_config_interrupt( GSEN_ACCEL_INT_THRESHOLD_SLEEP );/*使能GSEN外部中断*/
        power_man_save_state_set( gsen_power_man_id ,TASK_STOP_MODE );
        log_debug_send(INFOR_MODE,GSEN_MAN_NAME ,"<<<<<<<<<<*****GSEN task enters STOP mode*****>>>>>>>>>>\r\n", 0);
        ret = pdPASS;
    }
    else if( state == TASK_STOP_MODE )
    {
        ret = pdPASS;
    }
    return ret;
}

/** ****************************************************************************
* @remarks       static void gsen_data_handle_task( void * pvParameters )
* @brief         GSEN基础数据处理任务
* @param[in]     * pvParameters 任务参数
* @param[out]    无
* @return        无
* @attention     无
*******************************************************************************/
static void gsen_data_handle_task( void * pvParameters )
{   
    ( void ) pvParameters; 
    #define GSEN_WORK_MAX_TIME  ( 100 )
    task_modules_e gsen_run_st;
    volatile uint32_t gsen_work_time = 0;

    mon_reg_task_dog( 60 * GAOZ_TICK_PERIOD_S ,(uint8_t *)GSEN_MAN_NAME , TASK_SEM_YES ); /*注册任务看门狗*/
    power_man_task_reg((uint8_t * )GSEN_MAN_NAME,(uint8_t *)&gsen_power_man_id ); /*注册系统电源管理*/
    log_debug_send(DEBUG_MODE,GSEN_MAN_NAME ,"GSEN management task started successfully\r\n",0);
    for(;;)
    {
        mon_reset_task_dog();
        gsen_run_st = power_man_task_state_get( gsen_power_man_id );                /*获取任务运行的状态*/
        if(( gsen_run_st == TASK_MODE_INIT ) || ( gsen_run_st == TASK_RUN_MODE ))
        {   
            if( gsen_reg_init() == BMI160_OK )                                      /*加速度传感器初始化*/
            {   
                gsen_sensor_work_st_set( GSEN_WORK_ST_OK );                         /*加速度传感器初始化成功*/
                #if( GSEN_BMI160 == 1 ) 
                log_debug_printf(INFOR_MODE,GSEN_MAN_NAME ,"GSEN register initialized successfully,chip id=%d\r\n",gsen_cfg.chip_id);
                #endif
                #if( GSEN_LIS3DH == 1 ) 
                log_debug_send(INFOR_MODE,GSEN_MAN_NAME ,"GSEN register initialized successfully\r\n",0 );
                #endif
            }
            else
            {
                gsen_sensor_work_st_set( GSEN_WORK_ST_ERR );                        /*加速度传感器初始化失败*/
                log_debug_printf(ERROR_MODE,GSEN_MAN_NAME ,"[%s]GSEN register registration failed\r\n",sys_err_infor_pointer_get( 18 ));
                power_man_save_state_set( gsen_power_man_id ,TASK_RUN_MODE );
                gaoz_task_sleep( 5000 );
                continue;
            }
        }
        else if(( gsen_run_st == TASK_SLEEP_MODE ) || ( gsen_run_st == TASK_STOP_MODE ))
        {
            power_man_save_state_set( gsen_power_man_id ,TASK_SLEEP_MODE );
            gaoz_task_sleep( 1000 ); 
            continue;
        }
        else if( gsen_run_st == TASK_STOP_MODE )
        {
            power_man_save_state_set( gsen_power_man_id ,TASK_STOP_MODE );
            gaoz_task_sleep( 1000 ); 
            continue;
        }
        power_man_save_state_set( gsen_power_man_id ,TASK_MODE_INIT );
        
        for( ;; )
        {
            mon_reset_task_dog();
            gsen_run_st = power_man_task_state_get( gsen_power_man_id );        /*获取任务运行的状态*/
            /*******************************GSEN任务处于运行状态***********************************/
            if( gsen_run_st == TASK_RUN_MODE )
            {
                if( gsen_task_into_run_mode() != pdPASS )
                {
                    gaoz_task_sleep(1000);
                    continue;
                }
                if( gens_interrupt_semaphore_get() == 1 )
                {
                    if( gsen_sensor_interrupt_data_get() == pdPASS )
                    {
                        #if( GSEN_LIS3DH == 1 )
                        gsen_lis3dh_sensor_body_crash( &gsen_body_p );
                        #endif
                        #if( GSEN_BMI160 == 1 ) 
                        gsen_bmi160_sensor_body_crash( &gsen_body_p );
                        #endif
                    }
                }
                else
                {
                    if( gsen_sensor_data_get() == pdPASS )                      /*获取6轴传感器原始数据*/
                    {   
                        gsen_work_time = 0;
                        #if( GSEN_LIS3DH == 1 )
                        gsen_lis3dh_sensor_euler_angle( gsen_bmi160_info );
                        gsen_lis3dh_sensor_body_posture( gsen_bmi160_p );       /*判断车身的状态*/
                        #endif
                        #if( GSEN_BMI160 == 1 )
                        gsen_bmi160_sensor_euler_angle( gsen_bmi160_info );     /*计算欧拉角*/
                        gsen_bmi160_sensor_body_posture( gsen_bmi160_p );       /*判断车身的状态*/
                        #endif
                    }
                    else
                    {   
                        gsen_work_time ++;
                        if( gsen_work_time >= GSEN_WORK_MAX_TIME )              /*连续2S内没有收到数据后则异常 */
                        {
                            gsen_work_time = 0;
                            gsen_sensor_work_st_set( GSEN_WORK_ST_ERR );        /*加速度传感器初始化失败，重新进行加速度传感器初始化*/
                            break;
                        }
                    }
               }
            }
            /*******************************GSEM任务处于休眠状态************************************/
            else if( gsen_run_st == TASK_SLEEP_MODE )
            {
                if( gens_interrupt_semaphore_get() == 1 )
                {
                    if( gsen_sensor_interrupt_data_get() == pdPASS )
                    {
                        #if( GSEN_LIS3DH == 1 )
                        gsen_lis3dh_sensor_body_crash( &gsen_body_p );
                        #endif
                        #if( GSEN_BMI160 == 1 ) 
                        gsen_bmi160_sensor_body_crash( &gsen_body_p );
                        #endif
                    }
                }
                else
                {
                    gsen_task_into_sleep_mode();                                /*判断任务是否处于睡眠状态，关闭定时器打开外部中断*/
                }
            }
            /*******************************GSEN任务处于停止状态************************************/
            else if( gsen_run_st == TASK_STOP_MODE )
            {
                if( gsen_task_into_stop_mode() == pdPASS )                     /*判断任务是否处于停止状态，关闭定时器打开外部中断*/
                {
                    gaoz_task_sleep(1000);
                    continue;
                }
                gaoz_task_sleep(1000);
            }
            /*******************************GSEN任务空循环状态************************************/
            else 
            {
                gaoz_task_sleep(100);
            }
        }
    }
}


